#ifndef CONSTRAINT_H
#define CONSTRAINT_H

#include "Entity.h"
#include "Maths.h"

/*struct Segment {
    Vector* vPos1;
    Vector* vPos2;
};

typedef Vector* Point;*/

//TODO: Work out if can have array of objects of different types but all having a function in common without virtual functions

class IConstraint
{
public:
    virtual void update() = 0;
};

//Applies a physical distance constraint between two entities
class CLengthConstraint : public IConstraint
{
    
protected:
    
    friend class CWorld;
    
    CLengthConstraint(CPhysicsSegment segment, Scalar fLength)
    {
        init(segment, fLength);
    }
    
    void init(CPhysicsSegment segment, Scalar fLength);
    
public:
    
    virtual void update()
    {
        
        Vector vDelta = m_Segment.getVector();
    	Scalar fDeltaLength = vDelta.getLength();
    	Scalar fDiff = (fDeltaLength - m_fLength) / fDeltaLength;
    	
    	Scalar p = m_Segment.getPoint1()->getMass() / (m_Segment.getPoint1()->getMass() + m_Segment.getPoint2()->getMass());
    	
    	//TODO: See if should be using setPos() (i.e., should not be using Vector& getPos())
    	m_Segment.getPoint1()->getPos() += vDelta * 0.5 * fDiff * (1-p);
    	m_Segment.getPoint2()->getPos() -= vDelta * 0.5 * fDiff * (p);
        
    }
    
    //TODO: Remove this if satisfied it is not needed
    //Scalar getAngle()       { return (m_pPoints[1]->getPos() - m_pPoints[0]->getPos()).getAngle(); }
    
protected:
    
    CPhysicsSegment m_Segment;
    Scalar m_fLength;
    
};

//TODO: Consider rewriting this class without deriving it, as it was just conveniant, and the constructor is a tad of a hack
class CSpringConstraint : public CLengthConstraint
{
    
private:
    
    friend class CWorld;
    
    CSpringConstraint(CPhysicsSegment segment, Scalar fSpringConstant = 1.f, Scalar fDampingConstant = 0.f, Scalar fLength)
         : CLengthConstraint(segment, fLength), m_fSpringConstant(fSpringConstant), m_fDampingConstant(fDampingConstant)        {}
    
public:
    
    virtual void update();
    
private:
    
    Scalar m_fSpringConstant;
    Scalar m_fDampingConstant;
    
};

//TODO: Work out the best way to deal with pointers to points

class CSlideConstraint : public IConstraint
{

private:
    
    friend class CWorld;
    
    CSlideConstraint(CPhysicsPoint* pSlidingPoint, CSegment segment)
    {
        init(pSlidingPoint, segment);
    }
    
    void init(CPhysicsPoint* pSlidingPoint, CSegment segment);
    
public:
    
    virtual void update();
    
private:
    
    //TODO: Replace with segment structure
    CSegment            m_Segment;
    CPhysicsPoint*      m_pSlidingPoint;
    
};

#endif //CONSTRAINT_H
